   ! NOTE: This source file contains an example PitchCntrl() user-specified
   !       routine for computing blade pitch control commands based on transfer
   !       function descriptions specified in a pitch.ipt input file.  This
   !       routine was written by Craig Hansen (ACH) of Windward Engineering
   !       LLC.  Questions related to the use of this routine should be
   !       addressed to Craig Hansen.
module PitchCntrl_ACH
contains
!=======================================================================
SUBROUTINE PitchCntrl ( BlPitch, ElecPwr, LSS_Spd, TwrAccel, NB, ZTime, DT, DirRoot, TFOutput )


   ! This routine reads a data file containing user specified transfer
   !   function information to allow the use of control systems in FAST.
   !   The parameters read in, and array dimensions may be adjusted to 
   !   suit the users need.
   ! The transfer function coefficients are read in, then converted to
   !   state space form for integration using a fourth order Runge-Kutta
   !   integration scheme.
   ! This routine was originally written by C. Hansen in Fortran 77 for
   !   use with FAST.  It was converted to Modern Fortran by J. Jonkman.


USE                            NWTC_Library


IMPLICIT                        NONE


   ! Passed variables:

INTEGER   , INTENT(IN )      :: NB                                              ! Number of blades.

REAL(ReKi), INTENT(IN )      :: BlPitch (NB)                                    ! Current values of the blade pitch angles (rad)
REAL(DbKi), INTENT(IN )      :: DT                                              ! Integration time step (sec)
REAL(ReKi), INTENT(IN )      :: ElecPwr                                         ! Electrical power (watts)
REAL(ReKi), INTENT(IN )      :: LSS_Spd                                         ! LSS speed (rad/s)
REAL(ReKi), INTENT(OUT)      :: TFOutput(NB)                                    ! Desired pitch angles returned by this subroutine (rad)
REAL(ReKi), INTENT(IN )      :: TwrAccel                                        ! Tower top acceleration (m/s^2)
REAL(DbKi), INTENT(IN )      :: ZTime                                           ! Current simulation time (sec)

CHARACTER(1024), INTENT(IN ) :: DirRoot                                         ! The name of the root file including the full path to the current working directory.  This may be useful if you want this routine to write a permanent record of what it does to be stored with the simulation results: the results should be stored in a file whose name (including path) is generated by appending any suitable extension to DirRoot.


   ! Local Variables:

   ! NOTE: If the values of MSZ OR NSZ are changed, the PARAMETER
   !       statements in TFSISO must also be changed to match.
INTEGER   , PARAMETER        :: MSZ = 12                                        ! Larger than highest order of transfer function; also used to size number of constants CNST
INTEGER   , PARAMETER        :: NSZ =  4                                        ! Number of transfer functions we will use

REAL(ReKi)                   :: A0
REAL(ReKi), SAVE             :: AC     (MSZ,NSZ)
REAL(ReKi), SAVE             :: BC     (0:MSZ,NSZ)
REAL(ReKi), SAVE             :: CNST   (MSZ)                                    ! Maximum of MSZ constants
REAL(ReKi)                   :: TFInput                                         ! Input to the transfer function
REAL(ReKi), SAVE             :: TPCOn                                           ! Time to enable active pitch control.
REAL(ReKi)                   :: SUM

INTEGER   , SAVE             :: CntrlRgn                                        ! Control region (CntrlRgn = 2 = power control, CntrlRgn = 3 = speed control)
INTEGER                      :: I
INTEGER                      :: J
INTEGER                      :: NCNST
INTEGER   , SAVE             :: NORDER (NSZ)
INTEGER                      :: NR
INTEGER                      :: NTEMP

LOGICAL, SAVE                :: INITFLAG = .TRUE.

CHARACTER(80)                :: DESCRIP
CHARACTER( 3)                :: FmtText   = '(A)'                               ! Format for outputting pure text.

CHARACTER(1024)              :: PitchFileName                                   ! the name of the input control file
INTEGER(IntKi)               :: ErrStat
CHARACTER(ErrMsgLen)         :: ErrMsg


IF ( INITFLAG )  THEN


   ! Save the value of time in which pitch control is first activated:

   TPCOn = REAL( ZTime, ReKi)

   ! Read control parameters from 'pitch.ipt' if control is employed
   I = INDEX( DirRoot, PathSep, BACK=.TRUE. )
   IF ( I < LEN_TRIM(DirRoot) .OR. I > 0 ) THEN
      PitchFileName = DirRoot(1:I)//'pitch.ipt'
   ELSE
      PitchFileName = 'pitch.ipt'
   END IF
      
      
   CALL OpenFInpFile ( 86, TRIM(PitchFileName), ErrStat, ErrMsg )
   IF (ErrStat >= AbortErrLev) CALL ProgAbort(TRIM(ErrMsg))


   READ(86,FmtText) DESCRIP

   CALL WrScr1( '***********************************************' )
   CALL WrScr(  'Running with control option using data from:'    )
   CALL WrScr(  TRIM(DESCRIP)                                     )
   CALL WrScr(  '***********************************************' )
   CALL WrScr(  ' '                                                )

   READ(86,*) CntrlRgn


   READ(86,*) NCNST

   DO I = 1,NCNST
      READ(86,*,END=20) CNST(I)
   ENDDO

   NR = 1


10 READ(86,*,END=40) NTEMP ! Use NTEMP to avoid array overflow at end of file
   NORDER(NR) = NTEMP
   READ(86,*,END=30) (BC(I,NR), I = NORDER(NR),0,-1)
   READ(86,*,END=30) (AC(I,NR), I = NORDER(NR),1,-1), A0

   IF ( ABS(A0) > 1.E-7 )  THEN
      DO I = 0,NORDER(NR)
         BC(I,NR) = BC(I,NR)/A0
      ENDDO
      DO I = 1,NORDER(NR)
         AC(I,NR) = AC(I,NR)/A0
      ENDDO
   ELSE
      CALL ProgAbort ( 'Coefficient of largest power of s in the denominator must not be zero.' )
   ENDIF


   ! Calculate coefficients for state space model

   DO I = 1,NORDER(NR)
      SUM = 0.0
      DO J = 1,I
         SUM = SUM - AC(J,NR)*BC(I-J,NR)
      ENDDO
      BC(I,NR) = BC(I,NR) + SUM
   ENDDO


   NR = NR + 1


   GOTO 10

20 CALL WrScr( 'Error while reading constants from pitch.ipt file'     )
   CALL WrScr( 'Encountered end of file while reading constants'       )
   CALL WrScr( 'Number of constants expected: '//TRIM(Num2LStr(NCNST)) )
   CALL WrScr( 'Number of constants found:    '//TRIM(Num2LStr(I-1  )) )
   CALL ProgAbort( 'Check your pitch.ipt file.' )


30 CALL WrScr( 'Error in specification of transfer function #'//TRIM(Num2LStr(NR)) )
   CALL ProgAbort( 'Check your pitch.ipt file.' )


40 CONTINUE


   ! Always require that NSZ transfer functions are input to help
   !   ensure correct input file for this version of code

   IF( NR-1 /= NSZ )  THEN
      CALL WrScr( 'Error in pitch.ipt file'                           )
      CALL WrScr( 'Incorrect number of transfer functions '           )
      CALL WrScr( 'Number that were read:     '//TRIM(Num2LStr(NR-1)) )
      CALL WrScr( 'Number that were expected: '//TRIM(Num2LStr(NSZ )) )
      CALL ProgAbort( 'Check your pitch.ipt file.' )
   ENDIF


   INITFLAG = .FALSE.

   CLOSE(86)


   RETURN


ENDIF ! Initialization


   ! Determine the Transfer Function Input, TFInput, based on specified control
   !   region:

SELECT CASE ( CntrlRgn )   ! Which control region are we in?

CASE ( 2 )                 ! Region 2 control = power control

   TFInput = 0.001*ElecPwr             ! Electric power, kW

CASE ( 3 )                 ! Region 3 control = speed control

!   TFInput = HSS_Spd/ABS(GBRatio)*RPS2RPM   ! LSS speed at gearbox entrance, rpm
   TFInput = LSS_Spd*RPS2RPM                 ! LSS speed at gearbox entrance, rpm


CASE DEFAULT               ! None of the above

   CALL ProgAbort ( ' CntrlRgn must be 2 or 3.' )


ENDSELECT


   ! Use the control routine

CALL CTRL4 ( CNST, AC, BC, NORDER, MSZ, NSZ, &
             TFOutput, BlPitch, TFInput, TwrAccel, NB, ZTime, TPCOn )



RETURN
END SUBROUTINE PitchCntrl
!=======================================================================
SUBROUTINE CTRL4 ( CNST, AC, BC, NORDER, MSZ, NSZ, &
                   TFOutput, BlPitch, TFInput, TwrAccel, NB, ZTime, TPCOn )


   ! In this subroutine dependencies between transfer functions, as well
   !   as inputs and outputs of transfer functions are defined.  For the
   !   call to TFSISO the user need only be concerned with the first
   !   three arguments.  The first argument is the transfer fuction
   !   input, the second is the transfer function output, the third
   !   specifies the transfer function number, with the number
   !   coresponding to the order in which the transfer functions were
   !   read from the input file.


USE                            NWTC_Library

IMPLICIT                        NONE


   ! Passed variables:

INTEGER   , INTENT(IN )      :: MSZ
INTEGER   , INTENT(IN )      :: NB                                              ! Number of blades.
INTEGER   , INTENT(IN )      :: NSZ

REAL(ReKi), INTENT(IN )      :: AC      (MSZ,NSZ)
REAL(ReKi), INTENT(IN )      :: BC      (0:MSZ,NSZ)
REAL(ReKi), INTENT(IN )      :: BlPitch (NB)                                    ! Current blade pitch.
REAL(ReKi), INTENT(IN )      :: CNST    (MSZ)
REAL(ReKi), INTENT(IN )      :: TFInput
REAL(ReKi), INTENT(OUT)      :: TFOutput(NB)
REAL(ReKi), INTENT(IN )      :: TPCOn                                           ! Time to enable active pitch control.
REAL(ReKi), INTENT(IN )      :: TwrAccel
REAL(DbKi), INTENT(IN )      :: ZTime                                           ! Current simulation time.

INTEGER   , INTENT(IN )      :: NORDER  (NSZ)


   ! Local variables:

REAL(ReKi)                   :: AWIND    = 0.0
REAL(ReKi)                   :: DTSTRT
REAL(ReKi)                   :: DTCNTRL
REAL(ReKi)                   :: GAINSCHED
REAL(ReKi)                   :: GSCoef
REAL(ReKi)                   :: GSExp
REAL(ReKi)                   :: GSPit1
REAL(ReKi)                   :: GSPit2
REAL(ReKi)                   :: OLDTIME  = 0.0                                  ! Previous time we changed the pitch angle, sec
REAL(ReKi)                   :: OLDTFOUTPUT
REAL(ReKi), PARAMETER        :: OnePlusEps = 1.0 + EPSILON(OnePlusEps)         ! The number slighty greater than unity in the precision of ReKi.
REAL(ReKi)                   :: PHI0
REAL(ReKi)                   :: PHI1
REAL(ReKi)                   :: PHI2
REAL(ReKi)                   :: PITMAX
REAL(ReKi)                   :: PITMIN
REAL(ReKi)                   :: TWROUTPUT
REAL(ReKi)                   :: U1
REAL(ReKi)                   :: U2
REAL(ReKi)                   :: X

INTEGER                      :: DEBUGFLAG
INTEGER                      :: K                                               ! Blade number

LOGICAL                      :: TRIMFLAG =  .TRUE.                              ! Initialization flag

CHARACTER( 8)                :: Frmt1 = '(20(:A))'
CHARACTER(14)                :: Frmt2 = '(20(:G12.5,A))'
INTEGER(IntKi)               :: ErrStat
CHARACTER(ErrMsgLen)         :: ErrMsg


SAVE  ! mlb - Do we need to save everything?



   ! Enter variables which need to be initialized after trim solution here

IF ( TRIMFLAG )  THEN


   PHI0        = BlPitch(1)*R2D    ! Initial pitch angle (deg)
   OLDTFOUTPUT = BlPitch(1)
   TRIMFLAG    = .FALSE.


   ! Assign variable values from the pitch.ipt file

   PITMIN    = CNST( 4) ! Minimum pitch angle, deg
   PITMAX    = CNST( 5) ! Maximum pitch angle, deg
   DTCNTRL   = CNST( 6) ! Time interval for pitch control, sec
   GSPit1    = CNST( 7) ! Pitch angle for start of gain scheduling (>0), rad
   GSPit2    = CNST( 8) ! Pitch angle for end of gain scheduling, rad
   GSCoef    = CNST( 9) ! constant 'a' in gain schedule power law ( GS = a * x**p)
   GSExp     = CNST(10) ! exponent 'p' in gain schedule power law ( GS = a * x**p)
   DEBUGFLAG = CNST(11) ! Debug file output (0=no, 1=yes)


   ! Open file to receive control variable output for debug (if desired)

   IF( DEBUGFLAG == 1 ) THEN

      CALL OpenFOutFile (40, 'pitcntrl.plt', ErrStat, ErrMsg )
         IF (ErrStat >= AbortErrLev) CALL ProgAbort(TRIM(ErrMsg))
      WRITE (40,"( / 'This file was generated by ' , A , A , ' on ' , A , ' at ' , A , '.' / )")  &
                       TRIM(ProgName), TRIM( ProgVer ), CurDate(), CurTime()

      WRITE(40,*)  'Output of PITCH control control subroutine'
      WRITE(40,*)  'Gain schedule coeffs = ', GSCoef, GSExp

      WRITE(40,Frmt1)     &
      'Time',        TAB, &
      'Pitch',       TAB, &
      'TFInput',     TAB, &
      'TwrAccel',    TAB, &
      'TWROUTPUT',   TAB, &
      'Input1U1',    TAB, &
      'OutputPHI0',  TAB, &
      'OutputPHI1',  TAB, &
      'OutputPHI2',  TAB, &
      'PitchOutput', TAB, &
      'GainSched',   TAB, &
      'Awindup'

   ENDIF


ENDIF


!check for numerical stability
!print *, REAL(ZTime,ReKi) - OLDTIME, (REAL(ZTime,ReKi) - OLDTIME < DTCNTRL ) , EqualRealNos( REAL(ZTime,ReKi), OldTime+DtCntrl ) 
IF ( .NOT. EqualRealNos( REAL(ZTime,ReKi), OldTime+DtCntrl )  ) THEN
  IF( REAL(ZTime,ReKi) - OLDTIME < DTCNTRL )  THEN  ! Time check needed for FAST
!old IF( REAL(ZTime,ReKi) - OLDTIME < DTCNTRL )  THEN  ! Time check needed for FAST
!new: IF( ZTime*OnePlusEps - OLDTIME < DTCNTRL )  THEN  ! Time check needed for FAST
      DO K = 1,NB
         TFOutput(K) = OLDTFOUTPUT
      ENDDO ! K
      RETURN
   END IF
ENDIF


OLDTIME = REAL(ZTime,ReKi)


   ! Apply gain scheduling based on measured pitch angle (rad)

X = SAT2( BlPitch(1), GSPit1, GSPit2 ) ! Use endpoints if outside the endpoints
GAINSCHED = GSCoef*( X**GSExp )


   ! Ramp the gains up from zero to the desired values during startup
   ! The intent is to avoid controller problems due to startup transients

DTSTRT = ZTime - TPCOn + 0.01  ! Avoid zero gain by using offset
IF( DTSTRT < 5.0 )  GAINSCHED = GAINSCHED * DTSTRT / 5.0


   ! First TF input is error * gain

U1 = GAINSCHED * CNST(1) * ( CNST(2) - TFInput )
U2 = GAINSCHED * CNST(3) * AWIND


   ! U1   = Input to transfer function (rotor RPM or kW error, depending upon PCHMODE)
   ! PHI0 = Output of transfer function = pitch in degrees
   ! Third argument = number or ID of transfer function
   ! First transfer function is integral term of PID pitch demand control

CALL TFSISO( U1-U2, PHI0, 1, AC, BC, DTCNTRL, NORDER, MSZ, NSZ )


   ! Second transfer function is PD terms of PID pitch demand control
   ! NOTE: The antiwindup term is not included here

CALL TFSISO( U1   , PHI1, 2, AC, BC, DTCNTRL, NORDER, MSZ, NSZ )


   ! Add these two to get pitch demand control output

PHI1 = PHI0 + PHI1


   ! Now apply third transfer function to represent tower acceleration
   !   feedback.  If order of 3rd transfer function is zero, this
   !   transfer function is not applied.

IF ( NORDER(3) == 0 )  THEN
   TWROUTPUT = 0.0
ELSE
   CALL TFSISO( TwrAccel, TWROUTPUT, 3, AC, BC, DTCNTRL, NORDER, MSZ, NSZ )
ENDIF


   ! Add pitch demand from tower accel. to that from speed/power error

PHI1 = PHI1 + TWROUTPUT


   ! SAT2 function ensures that the pitch angle demand does not go
   !   beyond limits (2nd and 3rd arguments in degrees)

PHI2 = SAT2( PHI1, PITMIN, PITMAX ) ! Pitch angle, deg

AWIND = PHI1 - PHI2                 ! Anti windup term when pitch demand saturates, deg


   ! Now apply fourth transfer function to represent the actuator
   !   (If order of 4th transfer function is zero, this transfer function is not applied)

IF ( NORDER(4) == 0 )  THEN
   DO K=1,NB
      TFOutput(K) = PHI2
   ENDDO ! K
ELSE
   CALL TFSISO( PHI2, TFOutput(1), 4, AC, BC, DTCNTRL, NORDER, MSZ, NSZ )
   TFOutput = TFOutput(1)           ! All blades use same pitch in this version
ENDIF


DO K=1,NB
   TFOutput(K) = TFOutput(K)*D2R   ! Pitch angle returned by subroutine, rad
ENDDO ! K


OLDTFOUTPUT = TFOutput(1)           ! Save for use until next control time


   ! Write to controller output file if desired.

IF ( DEBUGFLAG == 1 )  THEN
   WRITE(40,Frmt2)          &
   ZTime,              TAB, &
   BlPitch(1)*R2D,     TAB, &
   TFInput,            TAB, &
   TwrAccel,           TAB, &
   TWROUTPUT,          TAB, &
   U1,                 TAB, &
   PHI0,               TAB, &
   PHI1,               TAB, &
   PHI2,               TAB, &
   TFOutput(1)*R2D,    TAB, &
   GAINSCHED,          TAB, &
   AWIND
ENDIF



RETURN
END SUBROUTINE CTRL4
!=======================================================================
SUBROUTINE TFSISO ( U, Y, NR, AC, BC, DT, NORDER, MSZ, NSZ )


   ! This routine integrates the transfer functions using a fourth order
   !   Runge-Kutta method.


USE                            NWTC_Library


IMPLICIT                        NONE


   ! Passed variables:

INTEGER   , INTENT(IN )      :: MSZ
INTEGER   , INTENT(IN )      :: NSZ

REAL(ReKi), INTENT(IN )      :: AC      (MSZ,NSZ)
REAL(ReKi), INTENT(IN )      :: BC      (0:MSZ,NSZ)
REAL(ReKi), INTENT(IN )      :: DT
REAL(ReKi), INTENT(IN )      :: U
REAL(ReKi), INTENT(OUT)      :: Y

INTEGER   , INTENT(IN )      :: NORDER  (NSZ)
INTEGER   , INTENT(IN )      :: NR


   ! Local variables:

   ! NOTE: If the values of M OR N are changed, the PARAMETER
   !       statements in PitchCntrl must also be changed to match.
INTEGER   , PARAMETER        :: M   = 12                                        ! Larger than highest order of transfer function; also used to size number of constants CNST
INTEGER   , PARAMETER        :: N   =  4                                        ! Number of transfer functions we will use

REAL(ReKi)                   :: DT6
REAL(ReKi)                   :: DXDT    (MSZ)
REAL(ReKi)                   :: DXM     (MSZ)
REAL(ReKi)                   :: DXT     (MSZ)
REAL(ReKi)                   :: HDT
REAL(ReKi)                   :: X       (M,N) = 0.0
REAL(ReKi)                   :: XT      (MSZ)

INTEGER                      :: I

LOGICAL                      :: INITFLAG(N)   = .TRUE.



IF ( INITFLAG(NR) )  THEN
   CALL TFINIT( U, Y, X, AC, BC, NORDER, NSZ, MSZ, NR )
   IF( ( NSZ /= N ) .OR. ( MSZ /= M ) )  THEN
      PRINT*,  'ERROR IN PARAMETERS M AND/OR N IN TFSISO'
   ENDIF
   INITFLAG(NR) = .FALSE.
ENDIF


HDT = 0.5*DT
DT6 = DT/6.0


DO I = 1,NORDER(NR)
   XT(I)   = X(I,NR)
ENDDO


CALL XDOT( U, XT, AC, BC, DXDT, NORDER, NSZ, MSZ, NR )
DO I = 1,NORDER(NR)
   XT(I)   = X(I,NR) + HDT*DXDT(I)
ENDDO


CALL XDOT( U, XT, AC, BC, DXT, NORDER, NSZ, MSZ, NR )
DO I = 1,NORDER(NR)
   XT(I)   = X(I,NR) + HDT*DXT (I)
ENDDO


CALL XDOT( U, XT, AC, BC, DXM, NORDER, NSZ, MSZ, NR )
DO I = 1,NORDER(NR)
   XT(I)   = X(I,NR) +  DT*DXM (I)
   DXM(I)  = DXT(I) + DXM(I)
ENDDO


CALL XDOT( U, XT, AC, BC, DXT, NORDER, NSZ, MSZ, NR )
DO I = 1,NORDER(NR)
   X(I,NR) = X(I,NR) + DT6*( DXDT(I) + DXT(I) +2.0*DXM(I) )
ENDDO


Y = X(1,NR) + BC(0,NR)*U



RETURN
END SUBROUTINE TFSISO
!=======================================================================
SUBROUTINE XDOT ( U, X, AC, BC, DXDT, NORDER, NSZ, MSZ, NR )


   ! This routine calculates derivatives for fourth order Runge-Kutta.


USE                            NWTC_Library


IMPLICIT                        NONE


   ! Passed variables:

INTEGER   , INTENT(IN )      :: MSZ
INTEGER   , INTENT(IN )      :: NSZ

REAL(ReKi), INTENT(IN )      :: AC      (MSZ,NSZ)
REAL(ReKi), INTENT(IN )      :: BC      (0:MSZ,NSZ)
REAL(ReKi), INTENT(OUT)      :: DXDT    (MSZ)
REAL(ReKi), INTENT(IN )      :: U
REAL(ReKi), INTENT(IN )      :: X       (MSZ)

INTEGER   , INTENT(IN )      :: NORDER  (NSZ)
INTEGER   , INTENT(IN )      :: NR


   ! Local variables:

REAL(ReKi)                   :: SUM

INTEGER                      :: I



   ! Derivatives:

DO I = 1,NORDER(NR)-1
   DXDT(I) = X(I+1) + BC(I,NR)*U
ENDDO


SUM = 0.0
DO I = 1,NORDER(NR)
   SUM     = SUM - X(I)*AC( NORDER(NR) + 1 - I, NR )
ENDDO


DXDT(NORDER(NR)) = SUM + BC(NORDER(NR),NR)*U



RETURN
END SUBROUTINE XDOT
!=======================================================================
SUBROUTINE TFINIT ( U, Y, X, AC, BC, NORDER, NSZ, MSZ, NR )


   ! This routine initialize states for fourth order Runge-Kutta.


USE                            NWTC_Library


IMPLICIT                        NONE


   ! Passed variables:

INTEGER   , INTENT(IN )      :: MSZ
INTEGER   , INTENT(IN )      :: NSZ

REAL(ReKi), INTENT(IN )      :: AC      (MSZ,NSZ)
REAL(ReKi), INTENT(IN )      :: BC      (0:MSZ,NSZ)
REAL(ReKi), INTENT(IN )      :: U
REAL(ReKi), INTENT(OUT)      :: X       (MSZ,NSZ)
REAL(ReKi), INTENT(IN )      :: Y

INTEGER   , INTENT(IN )      :: NORDER  (NSZ)
INTEGER   , INTENT(IN )      :: NR


   ! Local variables:

REAL(ReKi)                   :: SUM

INTEGER                      :: I



X(1,NR) = Y - BC(0,NR)*U
DO I = 1,NORDER(NR)-1
   X(I+1,NR) = -BC(I,NR)*U
ENDDO


SUM = 0.0
DO I = 1,NORDER(NR)-1
   SUM       = SUM - X(I,NR)*AC( NORDER(NR) + 1 - I, NR )
ENDDO


   ! Watch out for zero values of AC

IF( AC(1,NR) /= 0.0 )  THEN
   X(NORDER(NR),NR) = ( -SUM - BC(NORDER(NR),NR)*U )/AC(1,NR)
ELSE
   X(NORDER(NR),NR) = ( -SUM - BC(NORDER(NR),NR)*U )/0.001
ENDIF



RETURN
END SUBROUTINE TFINIT
!=======================================================================
FUNCTION SAT2 ( X, XMIN, XMAX )


   ! Saturation function.


USE                            NWTC_Library


IMPLICIT                        NONE


   ! Passed variables:

REAL(ReKi)                   :: SAT2
REAL(ReKi), INTENT(IN )      :: X
REAL(ReKi), INTENT(IN )      :: XMAX
REAL(ReKi), INTENT(IN )      :: XMIN



IF     ( X > XMAX )  THEN
   SAT2 = XMAX
ELSEIF ( X < XMIN )  THEN
   SAT2 = XMIN
ELSE
   SAT2 = X
ENDIF



RETURN
END FUNCTION SAT2
!=======================================================================
FUNCTION DEADBAND ( X, XMIN, XMAX )


   ! Deadband function.


USE                            NWTC_Library


IMPLICIT                        NONE


   ! Passed variables:

REAL(ReKi)                   :: DEADBAND
REAL(ReKi), INTENT(IN )      :: X
REAL(ReKi), INTENT(IN )      :: XMAX
REAL(ReKi), INTENT(IN )      :: XMIN



IF     ( X > XMAX )  THEN
   DEADBAND = X - XMAX
ELSEIF ( X < XMIN )  THEN
   DEADBAND = X - XMIN
ELSE
   DEADBAND = 0.0
ENDIF



RETURN
END FUNCTION DEADBAND
!=======================================================================
end module PitchCntrl_ACH
   